/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)

         PS: 二者需要设置相同的话题


    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号

    实现流程:
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 发布者 对象
        5.组织被发布的数据，并编写逻辑发布数据

*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include <sstream>
#include <mavros_msgs/RCIn.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Image.h>
#include "cv_bridge/cv_bridge.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"


double x_vel;
double z_angle_vel;

void RCCallBack(const mavros_msgs::RCIn::ConstPtr &msg) {
    //可以在这里接受遥控器的通道从而去做不同的事情
    mavros_msgs::RCIn rc;
    rc = *msg;
    x_vel = (rc.channels[1] - 1500) / 500.0;

    z_angle_vel = (rc.channels[0] - 1500) / 500.0;
    // ROS_INFO("channels 0: %d, x_vel: %.2lf", rc.channels[1], x_vel);
}

void ImageCallBack(const sensor_msgs::Image::ConstPtr &msg) {

    try {
        // 使用cv_bridge将ROS图像消息转换为灰度格式的OpenCV图像
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
        
        // 现在cv_ptr->image是灰度图，但可能看起来只有黑色和白色
        // 调整对比度
        double alpha = 2.0; // 增强的倍数
        double beta = 0;    // 常数项，用于调整亮度
        cv::Mat adjusted_image;
        cv_ptr->image.convertTo(adjusted_image, CV_32F, alpha, beta);
        
        // 将调整后的图像转换回8位格式
        cv::Mat gray_image;
        adjusted_image.convertTo(gray_image, CV_8U);
        
        // 显示调整后的灰度图像
        cv::imshow("Adjusted Grayscale Image Window", gray_image);
        cv::waitKey(30); // 等待30ms，以便OpenCV窗口可以更新
    }
    catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }
}


int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");

    //2.初始化 ROS 节点:命名(唯一)
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"talker");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能

    //4.实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    ros:: Subscriber rc_sub = nh.subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 10, RCCallBack);
    ros:: Subscriber image_sub = nh.subscribe<sensor_msgs::Image>("/car/usb_cam/image_raw", 10, ImageCallBack);
    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
    std_msgs::String msg;
    // msg.data = "你好啊！！！";
    std::string msg_front = "运行中 "; //消息前缀
    int count = 0; //消息计数器

    //逻辑(一秒10次)
    ros::Rate r(10);

    //节点不死
    while (ros::ok())
    {
        //使用 stringstream 拼接字符串与编号
        std::stringstream ss;
        ss << msg_front << count;
        msg.data = ss.str();
        //发布消息
        pub.publish(msg);
        //加入调试，打印发送的消息
        ROS_INFO("发送的消息:%s",msg.data.c_str());

        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = -x_vel;
        vel_msg.angular.z = z_angle_vel;
        cmd_vel_pub.publish(vel_msg);

        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
        count++;//循环结束前，让 count 自增
        //暂无应用
        ros::spinOnce();
    }


    return 0;
}
